#include <Arduino.h>
#include <i2cmaster.h>
//#include "cppfix.h"
#include "maxim.h"
#include "utils.h"
#include "PrintBuf.h"
#include "packdata.h"


boolean helloall()
{
 // helloall
 // simply send 0xE0 as a write.
 i2c_start_wait(HELLO_ALL_CMD + I2C_WRITE);
 i2c_stop();
 return true;
}


// return number of devices..should do this like
// readCellVoltages with a register class.
boolean rollcall(uint8_t& numDevices)
{
 uint8_t ret;
 int i=0;
 int numFF;

 // rollcall

 i2c_start_wait(BROADCAST_ADDRESS + I2C_WRITE);
 if (i2c_write(ROLLCALL_REGISTER)) {
     PrintBuf::singleton()->add("Write ROLLCALL!");
    return false;
 }

 i2c_rep_start(BROADCAST_ADDRESS + I2C_READ);

 // read till two consecutive 0xFF bytes are seen.
 for (numDevices = 0; ;numDevices++) {

    // read device address
    ret = i2c_readAck();
    PrintBuf::singleton()->add(ret);
    if (ret == 0xFF || ret == 0) {
      break;
    } 

    // ignore device value
    (void) i2c_readAck();
 }
 (void)i2c_readNak();
 i2c_stop();
 return true;
}


boolean setlastaddress(uint8_t numDevices)
{
  RegDeviceAddress rd(numDevices);
  return rd.setLastAddress();
}

boolean initBoard(PackData* packData)
{
  // assuming numDevices < 16
  uint8_t numDevices=0;
 
  Serial.println("i2cinit");
  i2c_init();

  Serial.println("hello all");
  if (!helloall()) {
    PrintBuf::singleton()->add("helloAll!");
    return false;    
  }
  
  if (!rollcall(numDevices)) {
    PrintBuf::singleton()->add("RollCall!.");
    return false;
  }  else {
    PrintBuf::singleton()->add("Devices",'='); 
    PrintBuf::singleton()->add(numDevices);
  }
  Serial.print("NumDevices=");Serial.println(numDevices);

 // ideally should verify devices and then set last address.
  if (!setlastaddress(numDevices)) {
    PrintBuf::singleton()->add("SetLastAddress!");
    return false;
  }

  // Initialize PackData singleton
  packData->clear();
  packData->setNumDevices(numDevices);

  // Clear Status register
  RegStatus regStatus(numDevices);
  if (!regStatus.clearStatus()) {
    PrintBuf::singleton()->add("ClearStatus!");
    return false;
  }

  return true;
}

